#include <stdio.h>
#include <stdlib.h>
#include <winsock2.h>
#include <jansson.h>
#include <windows.h>

#include "ethercat_motor_control.h"

#pragma comment(lib, "ws2_32.lib")
// #pragma comment(lib, "jansson.lib")


#define BUFFER_SIZE 1024

#define STATUS_UPDATE_INTERVAL 100 // 100 milliseconds

#ifndef ETHERCAT_MOTOR_CONTROL_H


#endif // ETHERCAT_MOTOR_CONTROL_H

// Mutex for thread-safe access to motor data
HANDLE motorMutex;

SOCKET clientSocket = INVALID_SOCKET;

// Function to parse JSON and update motor data
void parse_json_and_update_motor(const char* json_str) {
    json_error_t error;
    json_t* root = json_loads(json_str, 0, &error);

    if (!root) {
        printf("Failed to parse JSON: %s\n", error.text);
        return;
    }

    // Extract data from JSON
    json_t* json_slaveid = json_object_get(root, "slaveid");
    json_t* json_target_position = json_object_get(root, "target_position");
    json_t* json_velocity = json_object_get(root, "velocity");

    if (json_is_integer(json_slaveid)) {
        int slaveid = (int)(json_integer_value(json_slaveid));
        if (slaveid >= 1 && slaveid <= slave_count) {
            // Lock motor data for thread-safe access
            WaitForSingleObject(motorMutex, INFINITE);

            // Update motor data
            devices[slaveid - 1].target_position = json_is_integer(json_target_position) ? (int)(json_integer_value(json_target_position)) : 0;
            devices[slaveid - 1].velocity = json_is_integer(json_velocity) ? (int)(json_integer_value(json_velocity)) : 0;
            devices[slaveid - 1].is_running = 1; // Mark motor as running

            SV630N(slaveid, devices[slaveid - 1].target_position, devices[slaveid - 1].velocity);

            // // Unlock motor data
            ReleaseMutex(motorMutex);
        } else {
            printf("Invalid slaveid: %d\n", slaveid);
        }
    }

    json_decref(root);
}

// Thread function to handle client connections
DWORD WINAPI handle_client(LPVOID lpParam) {
    clientSocket = (SOCKET)lpParam;
    char buffer[BUFFER_SIZE];
    int bytesReceived;

    while ((bytesReceived = recv(clientSocket, buffer, BUFFER_SIZE, 0)) > 0) {
        buffer[bytesReceived] = '\0';
        printf("Received: %s\n", buffer);

        // Parse JSON and update motor data
        parse_json_and_update_motor(buffer);

        // Send acknowledgment back to client
        send(clientSocket, buffer, bytesReceived, 0);
    }

    if (bytesReceived == SOCKET_ERROR) {
        printf("Receive failed.\n");
    }

    closesocket(clientSocket);
    return 0;
}

// Thread function to report motor status
DWORD WINAPI report_motor_status(LPVOID lpParam) {
    
    while (1) {
        // Lock motor data for thread-safe access
        WaitForSingleObject(motorMutex, INFINITE);

        // Print status for all motors
        for (int i = 0; i < slave_count; i++) {
            printf("Motor %d: Position=%d, Speed=%d, Torque=%d, Target Position=%d\n",
                   devices[i].ethercatid,
                   devices[i].current_position,
                   devices[i].current_speed,
                   devices[i].current_torque,
                   devices[i].target_position);
            if(clientSocket!=INVALID_SOCKET){
                send(clientSocket, "1", 1, 0);
            }
        }

        // Unlock motor data
        ReleaseMutex(motorMutex);

        // Wait for the next status update
        Sleep(STATUS_UPDATE_INTERVAL);
    }
    return 0;
}

// Function to start the TCP server
void start_tcp_server(SOCKET serverSocket) {
    SOCKET clientSocket;
    struct sockaddr_in clientAddr;
    int clientAddrLen = sizeof(clientAddr);

    printf("Server is listening on port %d...\n", port);

    while (1) {
        if ((clientSocket = accept(serverSocket, (struct sockaddr*)&clientAddr, &clientAddrLen)) == INVALID_SOCKET) {
            printf("Accept failed.\n");
            continue; // Continue listening instead of exiting
        }

        printf("Client connected.\n");

        // Create a new thread to handle the client connection
        HANDLE hThread = CreateThread(NULL, 0, handle_client, (LPVOID)clientSocket, 0, NULL);
        if (hThread == NULL) {
            printf("Failed to create thread.\n");
            closesocket(clientSocket);
        } else {
            CloseHandle(hThread); // Close the thread handle as it is no longer needed
        }
    }
}

int main(int argc, char *argv[]){
    init_soem(argc, argv);
    WSADATA wsaData;
    SOCKET serverSocket;
    struct sockaddr_in serverAddr;

    // Initialize Winsock
    if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
        printf("WSAStartup failed.\n");
        return 1;
    }

    // Create server socket
    if ((serverSocket = socket(AF_INET, SOCK_STREAM, 0)) == INVALID_SOCKET) {
        printf("Socket creation failed.\n");
        WSACleanup();
        return 1;
    }

    // Configure server address
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_addr.s_addr = INADDR_ANY;
    serverAddr.sin_port = htons(11);

    // Bind the socket
    if (bind(serverSocket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) == SOCKET_ERROR) {
        printf("Bind failed.\n");
        closesocket(serverSocket);
        WSACleanup();
        return 1;
    }

    // Start listening
    if (listen(serverSocket, 5) == SOCKET_ERROR) {
        printf("Listen failed.\n");
        closesocket(serverSocket);
        WSACleanup();
        return 1;
    }

    // Create a mutex for thread-safe access to motor data
    motorMutex = CreateMutex(NULL, FALSE, NULL);
    if (motorMutex == NULL) {
        printf("Failed to create mutex.\n");
        closesocket(serverSocket);
        WSACleanup();
        return 1;
    }

    // Start the motor status reporting thread
    // HANDLE hStatusThread = CreateThread(NULL, 0, report_motor_status, NULL, 0, NULL);
    // if (hStatusThread == NULL) {
    //     printf("Failed to create status thread.\n");
    //     closesocket(serverSocket);
    //     WSACleanup();
    //     return 1;
    // }

    // Start the TCP server
    start_tcp_server(serverSocket);

    // Cleanup
    closesocket(serverSocket);
    WSACleanup();
    return 0;
}